#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "memPool.h"


#define MAX_MEMORY_BLOCKS 5

static memory_block_t memory_pool[MAX_MEMORY_BLOCKS];
static SemaphoreHandle_t memory_pool_mutex;
static memory_block_t memory_pool_mini[MAX_MEMORY_BLOCKS];
static SemaphoreHandle_t memory_pool_mini_mutex;

void init_memory_pool() 
{
    memory_pool_mutex = xSemaphoreCreateMutex();
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        memory_pool[i].buffer = malloc(4 * 1024);
        memory_pool[i].size = 4 * 1024;
        memory_pool[i].in_use = false;
    }
}

uint8_t* allocate_from_pool(size_t required_size, size_t *allocated_size) 
{
    xSemaphoreTake(memory_pool_mutex, portMAX_DELAY);
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        if (!memory_pool[i].in_use && memory_pool[i].size >= required_size) {
            memory_pool[i].in_use = true;
            *allocated_size = memory_pool[i].size;
            xSemaphoreGive(memory_pool_mutex);
            return memory_pool[i].buffer;
        }
    }
    
    xSemaphoreGive(memory_pool_mutex);
    return NULL;
}

void free_to_pool(uint8_t *buffer) 
{
    xSemaphoreTake(memory_pool_mutex, portMAX_DELAY);
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        if (memory_pool[i].buffer == buffer) {
            memory_pool[i].in_use = false;
            break;
        }
    }
    
    xSemaphoreGive(memory_pool_mutex);
}

void init_memory_pool_mini() 
{
    memory_pool_mini_mutex = xSemaphoreCreateMutex();
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        memory_pool_mini[i].buffer = malloc(4 * 128);
        memory_pool_mini[i].size = 4 * 128;
        memory_pool_mini[i].in_use = false;
    }
}

uint8_t* allocate_from_pool_mini(size_t required_size, size_t *allocated_size) 
{
    xSemaphoreTake(memory_pool_mini_mutex, portMAX_DELAY);
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        if (!memory_pool_mini[i].in_use && memory_pool_mini[i].size >= required_size) {
            memory_pool_mini[i].in_use = true;
            *allocated_size = memory_pool_mini[i].size;
            xSemaphoreGive(memory_pool_mini_mutex);
            return memory_pool_mini[i].buffer;
        }
    }
    
    xSemaphoreGive(memory_pool_mini_mutex);
    return NULL;
}

void free_to_pool_mini(uint8_t *buffer) 
{
    xSemaphoreTake(memory_pool_mini_mutex, portMAX_DELAY);
    
    for (int i = 0; i < MAX_MEMORY_BLOCKS; i++) {
        if (memory_pool_mini[i].buffer == buffer) {
            memory_pool_mini[i].in_use = false;
            break;
        }
    }
    
    xSemaphoreGive(memory_pool_mini_mutex);
}